

#include "findCornerPoint.hpp"


////大津
//int QtsuAlgThreshold( const Mat image)
//{
//    int T=0; //Otsu算法阈值
//    double varValue=0; //类间方差中间值保存
//    double w0=0; //前景像素点数所占比例
//    double w1=0; //背景像素点数所占比例
//    double u0=0; //前景平均灰度
//    double u1=0; //背景平均灰度
//    double Histogram[256]={0}; //灰度直方图，下标是灰度值，保存内容是灰度值对应的像素点总数
//    uchar *data=image.data;
//    double totalNum=image.rows*image.cols; //像素总数
//    //计算灰度直方图分布，Histogram数组下标是灰度值，保存内容是灰度值对应像素点数
//    for(int i=0;i<image.rows;i++)   //为表述清晰，并没有把rows和cols单独提出来
//    {
//        for(int j=0;j<image.cols;j++)
//        {
//            Histogram[data[i*image.step+j]]++;
//        }
//    }
//    for(int i=0;i<255;i++)
//    {
//        //每次遍历之前初始化各变量
//        w1=0;       u1=0;       w0=0;       u0=0;
//        //***********背景各分量值计算**************************
//        for(int j=0;j<=i;j++) //背景部分各值计算
//        {
//            w1+=Histogram[j];  //背景部分像素点总数
//            u1+=j*Histogram[j]; //背景部分像素总灰度和
//        }
//        if(w1==0) //背景部分像素点数为0时退出
//        {
//            break;
//        }
//        u1=u1/w1; //背景像素平均灰度
//        w1=w1/totalNum; // 背景部分像素点数所占比例
//        //***********背景各分量值计算**************************
//        
//        //***********前景各分量值计算**************************
//        for(int k=i+1;k<255;k++)
//        {
//            w0+=Histogram[k];  //前景部分像素点总数
//            u0+=k*Histogram[k]; //前景部分像素总灰度和
//        }
//        if(w0==0) //前景部分像素点数为0时退出
//        {
//            break;
//        }
//        u0=u0/w0; //前景像素平均灰度
//        w0=w0/totalNum; // 前景部分像素点数所占比例
//        //***********前景各分量值计算**************************
//        
//        //***********类间方差计算******************************
//        double varValueI=w0*w1*(u1-u0)*(u1-u0); //当前类间方差计算
//        if(varValue<varValueI)
//        {
//            varValue=varValueI;
//            T=i;
//        }
//    }
//    return T;
//    
//    
//}

int GetMeanThreshold1(const Mat image)
{
    
    int Sum = 0;
    int Amount = 0;
    int idx = 0;
    double Histogram[256]={0}; //灰度直方图，下标是灰度值，保存内容是灰度值对应的像素点总数
    uchar *data=image.data;
    for(int i=0;i<image.rows;i++)   //为表述清晰，并没有把rows和cols单独提出来
    {
        for(int j=0;j<image.cols;j++)
        {
            Histogram[data[i*image.step+j]]++;//*
        }
    }
    for (int Y = 70; Y < 255; Y++)
    {
        //        Amount += Histogram[Y];
        //        Sum += Y * Histogram[Y];
        Amount +=Histogram[Y];
    }
    Amount = Amount / 2;
    for(int Y =70;Y<255;Y++)
    {
        if(Sum < Amount)
        {
            Sum += Histogram[Y];
            idx = Y;
        }
    }
    return idx;
}
bool deletePoint1(vector<Point> &vecLinePoint, Mat srcImage)
{
    float slop1,slop2;
    std::vector<float> slop;
    int max = 0;
    float dist;
    dist = 0;
    for(int i = 0; i < vecLinePoint.size(); i++)
        for( int k = i+1; k < vecLinePoint.size() ; k++)
        {
            slop1 = (((float)vecLinePoint[i].y -(float) vecLinePoint[k].y )/((float) vecLinePoint[i].x- (float)vecLinePoint[k].x ));
            if(slop1 > 9999) slop1 =9999;
            slop.push_back(slop1);
            dist = dist +1;
        }
    slop2 = 0;
    for(int i=0; i < slop.size() - 1; ++i)
        if(max < count(slop.begin(), slop.end(), slop[i]))
        {
            max = count(slop.begin(), slop.end(), slop[i]);
            slop2 = slop[i];
            
        }
    int flag= 0;
    for(int i = 0; i < vecLinePoint.size() ; i++)
    {
        flag = 0;
        for(int k = 0; k < vecLinePoint.size() ; k++)
        {
            if(i != k)
            {
                slop1 = (((float)vecLinePoint[i].y -(float) vecLinePoint[k].y )/((float) vecLinePoint[i].x - (float)vecLinePoint[k].x ));
                if(slop1 > 9999) slop1 =9999;
                if(slop2== slop1)
                {
                    flag =1;
                }
            }
            
        }
        if( flag == 0)
        {
            vecLinePoint.erase(vecLinePoint.begin()+ i);
            i = i - 1;
            cout << "delete" << "  ";
        }
    }
    cout << endl;
    for(int  i = 0; i < vecLinePoint.size(); i++ )
        circle(srcImage,vecLinePoint[i],10,cv::Scalar( 128,255, 255 ),2,9,0);
    if(vecLinePoint.size() >=2)
        return true;
    else
        return false;
}

void LineFitLeastSquares1(vector<cv::Point> &vecLinePoint, float &k, float &dist)
{
    
    float A = 0.0;
    float B = 0.0;
    float C = 0.0;
    float D = 0.0;
    
    for (int i = 0; i < vecLinePoint.size(); i++)
    {
        A +=(float) vecLinePoint[i].x *(float) vecLinePoint[i].x;
        B += (float)vecLinePoint[i].x;
        C +=(float) vecLinePoint[i].x *(float) vecLinePoint[i].y;
        D += (float)vecLinePoint[i].y;
    }
    
    // º∆À„–±¬ k∫ÕΩÿæ‡dist
    float  temp;
    temp=(vecLinePoint.size()*A - B*B);
    if( temp > 0.000001)// ≈–∂œ∑÷ƒ∏≤ªŒ™0
    {
        k = (vecLinePoint.size()*C - B*D) / temp;
        dist = (A*D - B*C) / temp;
    }
    else
    {
        k = 0;
        dist = B / vecLinePoint.size();
    }
    
    
}


cv::Point2f FindCornerPoint::computeIntersect1(Line l1, Line l2)
{
    float x1 = l1._p1.x, x2 = l1._p2.x, y1 = l1._p1.y, y2 = l1._p2.y;
    float x3 = l2._p1.x, x4 = l2._p2.x, y3 = l2._p1.y, y4 = l2._p2.y;
    float k1,k2,b1,b2;
    cv::Point2f pt;
    k1=(y1 - y2) / (x1 - x2);
    b1= y1 - k1 * x1;
    if (x3 != x4) {
        k2=(y3 - y4) / (x3 - x4);
        b2= y3 - k2 * x3;
        pt.x = (b1 - b2) / ( k2 - k1);
    }
    else{
        pt.x = x3;
    }
    pt.y = k1 * pt.x + b1;
    return pt;
    
    
}



bool FindCornerPoint::getCornerInfo(cv::Mat srcImage, cv::Point &cornerPosition, float &turnAngle)
{
    CornerPlace cornerPlace = upRight;
    Line HorizontalLine, VerticalLine;
    int imageWidth = srcImage.cols;
    int imageHeight = srcImage.rows;
    int RectWidth = 10;
    int RectHeight = 10;
    cv::Vec4f Hline, Vline;
    
    cv::Rect rectHorizontal, rectVertical;
    rectHorizontal.width = RectWidth; rectHorizontal.height = RectHeight;
    rectVertical.width = RectHeight; rectVertical.height = RectWidth;
    
    std::vector<cv::Point> vecLinePoint;
    int thresh1;
    //thresh1 = QtsuAlgThreshold(srcImage);
    thresh1 = GetMeanThreshold1(srcImage);
    thresh1 *= 1;//二值化阈值
    if(thresh1<20)
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    
    }
    int lastSum, currentSum;
    int rowLoop, colLoop;
    int idx;
    int Sum = 0;
    cv::Point temp;//
    //int thresh = RectWidth*RectHeight * 50;//垂直边响应阈值
    int thresh = RectWidth * RectHeight * 255 * 3000 / (thresh1 * thresh1);
//    int thresh2 = RectWidth*RectHeight * 25;//水平边响应阈值
    int thresh2 = RectWidth * RectHeight * 255 * 1600 / (thresh1 * thresh1);
    Mat srcImage1;
    srcImage1 = srcImage.clone();
    threshold(srcImage1,srcImage1,thresh1,255,THRESH_BINARY);
//    resize(srcImage1, srcImage1, Size(800,800));
//    imshow("aa",srcImage1);
//    waitKey(0);
    

    
    vecLinePoint.clear();
    //vertical
    for (idx = 1; idx < 19; idx++)
    {
        //	rectVertical.y = imageHeight / 2 + (3 - idx) * 200;
            rectVertical.y = imageHeight - idx * 20;
        
        if (cornerPlace == upRight || cornerPlace == downRight)
        {
            for (colLoop = 2000; colLoop > 10; colLoop--)
            {
                rectVertical.x = colLoop;
                lastSum = cv::sum(srcImage1(rectVertical))[0];
//                rectVertical.x = colLoop;
//                currentSum = cv::sum(srcImage(rectVertical))[0];
//                Sum = currentSum - lastSum;
 //               cout<<"坐标点："<<rectVertical.x<<" , "<<rectVertical.y<<"  Sum:"<<lastSum<<endl;
                if (lastSum > thresh)
                {
                    temp.x = colLoop;
                    temp.y = rectVertical.y;
                    break;
                }
            }//for
        }//if
        else
        {
            for (colLoop = 10; colLoop < imageWidth / 3; colLoop++)
            {
                rectVertical.x = colLoop;
                currentSum = cv::sum(srcImage(rectVertical))[0];
                rectVertical.x = colLoop - rectVertical.width;
                lastSum = cv::sum(srcImage(rectVertical))[0];
                Sum = currentSum - lastSum;
                if (Sum > thresh)
                {
                    temp.x = colLoop;
                    temp.y = rectVertical.y;
                    break;
                }
                
            }
        }//else
       
        circle(srcImage,temp,10,cv::Scalar(  255 ),2,9,0);
        
        vecLinePoint.push_back(temp);
    }//for count
    
    if(vecLinePoint.size() >= 2)
        deletePoint1(vecLinePoint,srcImage);
    else
        {
            turnAngle = 0;
            cornerPosition.x = 0;
            cornerPosition.y = 1024;
            std::cout << turnAngle << std::endl;
            std::cout << cornerPosition << std::endl;
            return true;
        }
        
    
    if (vecLinePoint.size()>= 2)
    {
        cv::fitLine(vecLinePoint, Vline, CV_DIST_HUBER, 0, 0.01, 0.01);
        VerticalLine._p1.y = (imageHeight >> 1) - 800;
        VerticalLine._p1.x = (VerticalLine._p1.y - Vline[3])*Vline[0] / Vline[1] + Vline[2];
        VerticalLine._p2.y = (imageHeight >> 1) + 800;
        VerticalLine._p2.x = (VerticalLine._p2.y - Vline[3])*Vline[0] / Vline[1] + Vline[2];
    }
    else
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    
    
    //horizontal
    vecLinePoint.clear();
    for (idx = 12; idx < 25; idx++)
    {
        if (cornerPlace == upRight || cornerPlace == downRight)
        {
            if(VerticalLine._p1.y < VerticalLine._p2.y )
                rectHorizontal.x = VerticalLine._p1.x - idx * 17;
            else
                rectHorizontal.x = VerticalLine._p2.x - idx * 17;
            
        }
        //rectHorizontal.x = idx * 100;
        else
            rectHorizontal.x = VerticalLine._p1.x + idx * 10;
        if (rectHorizontal.x < 0 || rectHorizontal.x > VerticalLine._p2.x)
            break;
        if (cornerPlace == upRight || cornerPlace == upLeft)
        {
            for (rowLoop = 10; rowLoop < imageHeight-20; rowLoop++)
            {
                rectHorizontal.y = rowLoop;
                currentSum = cv::sum(srcImage1(rectHorizontal))[0];
                //cout<<"坐标点："<<rectHorizontal.x<<" , "<<rectHorizontal.y<<"  Sum:"<<currentSum<<endl;
                //rectHorizontal.y = rowLoop - RectHeight;
               // lastSum = cv::sum(srcImage(rectHorizontal))[0];
               // Sum = currentSum - lastSum;
                if (currentSum > thresh2)
                {
                    temp.x = rectHorizontal.x;
                    temp.y = rowLoop+10;
                    break;
                }
                
            }
        }//if
        else
        {
            for (rowLoop = imageHeight - 10; rowLoop > imageHeight / 3; rowLoop--)
            {
                rectHorizontal.y = rowLoop;
                lastSum = cv::sum(srcImage(rectHorizontal))[0];
                rectHorizontal.y = rowLoop - rectHorizontal.height;
                currentSum = cv::sum(srcImage(rectHorizontal))[0];
                Sum = currentSum - lastSum;
                if (Sum > thresh2)
                {
                    temp.x = rectHorizontal.x;
                    temp.y = rowLoop;
                    break;
                }
            }
        }//else
        circle(srcImage,temp,10,cv::Scalar(  255 ),2,9,0);
        vecLinePoint.push_back(temp);
        //
        
    }//for
    
    if(vecLinePoint.size() >= 2)
        deletePoint1(vecLinePoint,srcImage);
    else
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    float slop1,dist;
    if (vecLinePoint.size() >= 2)
    {
        LineFitLeastSquares1( vecLinePoint,slop1,dist);
        HorizontalLine._p1.x = (imageWidth >> 1) - 300;
        HorizontalLine._p1.y = (HorizontalLine._p1.x ) * slop1 + dist;
        HorizontalLine._p2.x = (imageWidth >> 1) + 700;
        HorizontalLine._p2.y = (HorizontalLine._p2.x ) * slop1 + dist;
        
    }
    else
        {
            turnAngle = 0;
            cornerPosition.x = 0;
            cornerPosition.y = 1024;
            std::cout << turnAngle << std::endl;
            std::cout << cornerPosition << std::endl;
            return true;
        }
    
    
    cv::line(srcImage, cv::Point(VerticalLine._p1.x, VerticalLine._p1.y), cv::Point(VerticalLine._p2.x, VerticalLine._p2.y), cv::Scalar(255, 255, 255), 4, CV_AA);
    cv::line(srcImage, cv::Point(HorizontalLine._p1.x, HorizontalLine._p1.y), cv::Point(HorizontalLine._p2.x, HorizontalLine._p2.y), cv::Scalar(255, 255, 255), 4, CV_AA);
    resize(srcImage, srcImage, Size(1024, 618));
    
    
    cv::Point2f p1 = computeIntersect1(VerticalLine,HorizontalLine);
    cv::Point2f p2, p3, p4;
    if (cornerPlace == upLeft || cornerPlace == upRight)
    {
        p2 = cv::Point2f(p1.x, p1.y + 1500);
        p3 = cv::Point2f(p1.x + 600, p2.y);
        Line strghtLine = Line(p2, p3);
        p4 = computeIntersect1(VerticalLine,strghtLine);
        if (p4.x > p2.x)
        {
            double tan_theta = (double)(p4.x - p2.x) / 1500;
            turnAngle = std::atan(tan_theta) * 180 / PI;
        }
        else if (p4.x < p2.x)
        {
            double tan_theta = (double)(p2.x - p4.x) / 1500;
            turnAngle = -std::atan(tan_theta) * 180 / PI;
        }
        else
            turnAngle = 0;
        
    }
    
    if (cornerPlace == downLeft || cornerPlace == downRight)
    {
        p2 = cv::Point2f(p1.x, p1.y - 1500);
        p3 = cv::Point2f(p1.x + 600, p2.y);
        Line strghtLine = Line(p2, p3);
        p4 = computeIntersect1(VerticalLine,strghtLine);
        if (p4.x > p2.x)
        {
            double tan_theta = (double)(p4.x - p2.x) / 1500;
            turnAngle = std::atan(tan_theta) * 180 / PI;
        }
        else if (p4.x < p2.x)
        {
            double tan_theta = (double)(p2.x - p4.x) / 1500;
            turnAngle = -std::atan(tan_theta) * 180 / PI;
        }
        else
            turnAngle = 0;
        
    }
    
    
    if (cornerPlace == upLeft)
        cornerPosition = p1;
    else if (cornerPlace == upRight)
        cornerPosition = cv::Point(-p1.x + imageWidth / 2, p1.y - imageHeight / 2);
    else if (cornerPlace == downLeft)
        cornerPosition = cv::Point(p1.x, imageHeight - p1.y);
    else
        cornerPosition = cv::Point(imageWidth - p1.x, imageHeight - p1.y);
    
    
    
    std::cout << turnAngle << std::endl;
    std::cout << cornerPosition << std::endl;
    if(abs(cornerPosition.x)<1297 && abs(cornerPosition.y) < 1025)
        return true;
    else return false;
}


bool FindCornerPoint::getCornerInfo1(cv::Mat srcImage, cv::Point &cornerPosition, float &turnAngle)
{
    Line VerticalLine,HorizontalLine;
    int Image_Width = srcImage.cols;
    int Image_Height = srcImage.rows;
    int thresh = GetMeanThreshold1(srcImage);
    thresh *= 1.3;
    if(thresh<20)
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    cv::Mat srcImage1 = srcImage.clone();
    threshold(srcImage1, srcImage1, thresh, 255, THRESH_BINARY);
    
    int rectWidth = 10;
    int rectHeight = 10;
    cv::Rect rectHorizontal,rectVertical;
    rectHorizontal.width = rectWidth;rectHorizontal.height = rectHeight;
    rectVertical.width = rectWidth;rectVertical.height = rectHeight;
    int boxSum = 0;
    int thresh1 = rectHeight * rectWidth * 255 * 1600 / (thresh * thresh);
    int thresh2 = rectHeight * rectWidth * 70;
    cv::Point temp;
    std::vector<cv::Point> vecLinePoint,horLinePoint;
    
    //search horizontal line
    for(int idx = 5; idx < 15;++idx)
    {
        rectHorizontal.x = Image_Width/2 - idx * 15 ;
        for(int rowLoop = Image_Height/2 - 400 ; rowLoop<Image_Height/2 +900 ; ++rowLoop)
        {
            rectHorizontal.y = rowLoop;
            boxSum = cv::sum(srcImage1(rectHorizontal))[0];
              //   std::cout<<"x: "<<rectHorizontal.x<<" y: "<<rectHorizontal.y<<" boxSum: "<<boxSum<<std::endl;
            if(boxSum >thresh1)
            {
                temp.x = rectHorizontal.x;
                temp.y = rectHorizontal.y +10;
                break;
            }
        }
        circle(srcImage, temp, 10, Scalar::all(255),2,9,0);
        horLinePoint.push_back(temp);
    }
    if(horLinePoint.size() >= 2)
    {
        deletePoint1(horLinePoint, srcImage);
    }
    else
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    Vec4f Hline,Vline;
    float slop = 0.0;
    float dist =0.0;
    if(horLinePoint.size() >= 2)
    {
        //LineFitLeastSquares1( horLinePoint,slop,dist);
        cv::fitLine(horLinePoint, Hline, CV_DIST_HUBER, 0, 0.01, 0.01);
        HorizontalLine._p1.x = (Image_Width >> 1) -300;
        HorizontalLine._p1.y = (HorizontalLine._p1.x - Hline[2]) *Hline[1]/Hline[0] +Hline[3];
        HorizontalLine._p2.x = (Image_Width >> 1) +700;
        HorizontalLine._p2.y = (HorizontalLine._p2.x - Hline[2]) *Hline[1]/Hline[0] +Hline[3];
    }
    else
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    for(int idx = 1; idx < 11;++idx)
    {
        rectVertical.y = HorizontalLine._p1.y + idx * 5;
        for(int colLoop = Image_Width/2 - 200; colLoop < Image_Width/2 +400; ++colLoop)
        {
            rectVertical.x = colLoop;
            boxSum = cv::sum(srcImage1(rectVertical))[0];
            //std::cout<<"x: "<<rectVertical.x<<" y: "<<rectVertical.y<<" Sum: "<<boxSum<<std::endl;
            if(boxSum > thresh2)
            {
                temp.x = rectVertical.x + 10;
                temp.y = rectVertical.y;
                break;
            }
        }
        circle(srcImage, temp, 10, Scalar::all(255),2,9,0);
        vecLinePoint.push_back(temp);
    }
    if(vecLinePoint.size() >= 2)
    {
        deletePoint1(vecLinePoint, srcImage);
    }
    else
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    if(vecLinePoint.size() >= 2)
    {
        cv::fitLine(vecLinePoint, Vline, CV_DIST_HUBER, 0, 0.01, 0.01);
        VerticalLine._p1.y = (Image_Height >> 1) - 800;
        VerticalLine._p1.x = (VerticalLine._p1.y - Vline[3])*Vline[0] / Vline[1] + Vline[2];
        VerticalLine._p2.y = (Image_Height >> 1) + 800;
        VerticalLine._p2.x = (VerticalLine._p2.y - Vline[3])*Vline[0] / Vline[1] + Vline[2];
    }
    else
    {
        turnAngle = 0;
        cornerPosition.x = 0;
        cornerPosition.y = 1024;
        std::cout << turnAngle << std::endl;
        std::cout << cornerPosition << std::endl;
        return true;
    }
    cv::line(srcImage, cv::Point(VerticalLine._p1.x, VerticalLine._p1.y), cv::Point(VerticalLine._p2.x, VerticalLine._p2.y), cv::Scalar(255, 255, 255), 4, CV_AA);
    cv::line(srcImage, cv::Point(HorizontalLine._p1.x, HorizontalLine._p1.y), cv::Point(HorizontalLine._p2.x, HorizontalLine._p2.y), cv::Scalar(255, 255, 255), 4, CV_AA);
    resize(srcImage, srcImage, Size(1024, 618));
    cv::Point2f p1 = computeIntersect1(HorizontalLine, VerticalLine);
    cv::Point2f p2, p3, p4;
    p2 = cv::Point2f(p1.x, p1.y + 1500);
    p3 = cv::Point2f(p1.x + 600, p2.y);
    Line strghtLine = Line(p2, p3);
    p4 = computeIntersect1(strghtLine, VerticalLine);
    if (p4.x > p2.x)
    {
        double tan_theta = (double)(p4.x - p2.x) / 1500;
        turnAngle = std::atan(tan_theta) * 180 / PI;
    }
    else if (p4.x < p2.x)
    {
        double tan_theta = (double)(p2.x - p4.x) / 1500;
        turnAngle = -std::atan(tan_theta) * 180 / PI;
    }
    else
        turnAngle = 0;
    cornerPosition = cv::Point(-p1.x + Image_Width / 2, p1.y - Image_Height / 2);
    std::cout << turnAngle << std::endl;
    std::cout << cornerPosition << std::endl;
    if(abs(cornerPosition.x)<1297 && abs(cornerPosition.y) < 1025)
        return true;
    else return false;
    
    
    
    
    
    
    
    
    return true;
}



